#include "slep.h"
#include "hr_time.h"
#include "epph.h"

FUNVAL LeastC(double *A, double * y, double z, OPTS opt);
FUNVAL LeastCm1(double *A, double * y, double z, OPTS opt);
FUNVAL LeastCm2(double *A, double * y, double z, OPTS opt);


FUNVAL LeastC(double *A, double * y, double z, OPTS opt){
	if(opt.lFlag == 0)
		return LeastCm1(A,y,z,opt);
	else if ( opt.lFlag == 1)
		return LeastCm2(A,y,z,opt);
}

FUNVAL LeastCm1(double *A, double * y, double z, OPTS opt){
	int m,n, iterStep,steps;
	FUNVAL funret;
	double rsL2,root, L, alphap, alpha, lambda0,beta, l_sum, r_sum, fVal, fValp, norm_xp, norm_xxp;
	double *ATy, *x, *Ax, *xp,*Axp,*xxp, *s, *As, *ATAs, *g, *v, *Av, *funVal, *ValueL, *Axy;

	m = opt.m;
	n = opt.n;


	x = (double *) malloc( sizeof(double) * n);
	xp = (double *) malloc( sizeof(double) * n);
	xxp = (double *) malloc( sizeof(double) * n);
	Ax = (double *) malloc( sizeof(double) * m);
	Axp = (double *) malloc( sizeof(double) * m);
	ATy = (double *) malloc( sizeof(double) * n);

	s = (double *) malloc( sizeof(double) * n);
	As = (double *) malloc( sizeof(double) * m);
	ATAs = (double *) malloc( sizeof(double) * n);

	g = (double *) malloc( sizeof(double) * n);
	v = (double *) malloc( sizeof(double) * n);
	Av = (double *) malloc( sizeof(double) * m);
	Axy = (double *) malloc( sizeof(double) * n);

	ValueL = (double *) malloc( sizeof(double) * opt.maxIter);
	funVal = (double *) malloc( sizeof(double) * opt.maxIter);

	initNormalization(&opt,A);

	normalizedmv('T',&opt,A,y,ATy);

	rsL2 = opt.rsL2;

	if(opt.init == 2)
		memset(x,0,sizeof(double)*n);
	else{
		if ( hasGot(&opt,"x0"))
			dcopy(n,opt.x0,1,x,1);
		else
			dcopy(n,ATy,1,x,1);
	}

	normalizedmv('N',&opt,A,x,Ax);

	// The Armijo Goldstein Line Search Scheme

	L = 1.0 + rsL2;

	// xp = x; Axp = Ax; xxp = zeros;
	dcopy(n,x,1,xp,1);
	dcopy(m,Ax,1,Axp,1);
	memset(xxp,0,sizeof(double)*n);

	alphap = 0.0;
	alpha = 1.0;

	lambda0 = 0.0;

	for( iterStep = 0 ; iterStep < opt.maxIter ; iterStep++){
		beta = (alphap - 1.0)/alpha;
		// s = x + beta * xxp;
		dcopy(n,x,1,s,1);
		daxpy(n,beta,xxp,1,s,1);

		// As = Ax + beta * (Ax - Axp);
		dcopy(m,Ax,1,As,1);
		dscal(m,1.0 + beta, As,1);
		daxpy(m,-beta,Axp,1,As,1);

		normalizedmv('T',&opt,A,As,ATAs);

		// g = ATAs - ATy + rsL2 * s;
		dcopy(n,ATAs,1,g,1);
		daxpy(n,-1.0,ATy,1,g,1);
		daxpy(n,rsL2,s,1,g,1);

		// xp = x; Axp = Ax;
		dcopy(n,x,1,xp,1);
		dcopy(m,Ax,1,Axp,1);

		for(;;){
			// v= s - g/L;
			dcopy(n,s,1,v,1);
			daxpy(n,-1.0/L,g,1,v,1);

			eplb(x,&root,&steps,v,n,z,lambda0);
			lambda0 = root;

			// v = x - s;
			dcopy(n,x,1,v,1);
			daxpy(n,-1.0,s,1,v,1);
			
			normalizedmv('N',&opt,A,x,Ax);

			// Av = Ax - As;
			dcopy(m,Ax,1,Av,1);
			daxpy(m,-1.0,As,1,Av,1);

			r_sum = ddot(n,v,1,v,1);
			l_sum = ddot(m,Av,1,Av,1);

			if( l_sum <= r_sum * (L - rsL2) )
				break;
			else
				L = maxof2(2*L , l_sum/r_sum + rsL2);
		}

		ValueL[iterStep] = L;
		alphap = alpha;
		alpha =  (1.0 + sqrt(4.0 * alpha * alpha + 1))/2.0;

		// xxp = x - xp; Axy = Ax - y;
		dcopy( n, x,1,xxp,1);
		daxpy(n, -1.0 ,xp,1,xxp,1);

		dcopy(m,Ax,1,Axy,1);
		daxpy(m,-1.0,y,1,Axy,1);

		fValp = fVal = funVal[iterStep] = ddot(m,Axy,1,Axy,1)/2.0 + ddot(n,x,1,x,1)*rsL2 / 2.0;
		if(iterStep != 0)fValp = funVal [ iterStep -1];

		norm_xp = dnrm2(n,xp,1);
		norm_xxp = dnrm2(n,xxp,1);
		if(terminationCondition(&opt,fVal,fValp,x,norm_xxp,norm_xp,iterStep))
			break;
	}

	funret.errorCode = 0;
	funret.funVal = funVal;
	funret.totIter = iterStep;
	strcpy(funret.type,"LeastC");
	funret.x = x;
	funret.ValueL = ValueL;
	return funret;


}



FUNVAL LeastCm2(double *A, double * y, double z, OPTS opt){
	int m,n, iterStep,steps;
	FUNVAL funret;
	double rsL2,root, L, gamma, alphap, alpha, lambda0,beta, l_sum, r_sum, fVal, fValp, norm_xp, norm_xxp, tao;

	double *x, *xp, *xxp, *Ax, *ATy, *Axp, *ATAx, *ATAxp, *s, *As, *ATAs, *g, *v, *xnew, *Axnew, *Av, *Axy, *funVal, *ValueL;

	m = opt.m;
	n = opt.n;


	x = (double * ) malloc( sizeof(double) * n);
	xp = (double * ) malloc( sizeof(double) * n);
	xxp = (double * ) malloc( sizeof(double) * n);

	Ax = (double * ) malloc( sizeof(double) * m);
	Axp = (double * ) malloc( sizeof(double) * m);
	ATy = (double * ) malloc( sizeof(double) * n);
	ATAx = (double * ) malloc( sizeof(double) * n);
	ATAxp = (double * ) malloc( sizeof(double) * n);

	s = (double * ) malloc( sizeof(double) * n);
	As = (double * ) malloc( sizeof(double) * m);
	ATAs = (double * ) malloc( sizeof(double) * n);

	g = (double * ) malloc( sizeof(double) * n);

	v = (double * ) malloc( sizeof(double) * n);
	xnew = (double * ) malloc( sizeof(double) * n);

	Axnew = (double * ) malloc( sizeof(double) * m);
	Av = (double * ) malloc( sizeof(double) * m);
	Axy = (double * ) malloc( sizeof(double) * m);

	ValueL = (double * ) malloc( sizeof(double) * opt.maxIter);
	funVal = (double * ) malloc( sizeof(double) * opt.maxIter);

	initNormalization(&opt,A);

	normalizedmv('T',&opt,A,y,ATy);

	rsL2 = opt.rsL2;

	if(opt.init == 2)
		memset(x,0,sizeof(double)*n);
	else{
		if ( hasGot(&opt,"x0"))
			dcopy(n,opt.x0,1,x,1);
		else
			dcopy(n,ATy,1,x,1);
	}

	normalizedmv('N',&opt,A,x,Ax);

	// Adaptive Line Search;

	L = 1.0 + rsL2;

	lambda0 = 0.0;

	gamma = 1.0;

	dcopy(n,x,1,xp,1);
	dcopy(m,Ax,1,Axp,1);

	memset(xxp,0,sizeof(double)*n);

	normalizedmv('T',&opt,A,Ax,ATAx);

	for( iterStep =0; iterStep <opt.maxIter ; iterStep++){
		dcopy(n,ATAx,1,ATAxp,1);
		if(iterStep != 0)
			normalizedmv('T',&opt,A,Ax,ATAx);
		// Line search for L
		for(;;){
			if(iterStep != 0){
				alpha = ( - gamma  + sqrt(gamma*gamma + 4*L*gamma))/ (2*L);
				beta = (gamma - gamma * alphap)/(alphap * gamma + alphap * L * alpha);
				// s = x + beta * xxp;
				dcopy(n,x,1,s,1);
				daxpy(n,beta,xxp,1,s,1);

				// As = Ax+ beta * (Ax - Axp);
				dcopy(m,Ax,1,As,1);
				dscal(m,1.0 + beta , As,1);
				daxpy(m,-beta,Axp,1,As,1);

				// ATAs = ATAx + beta * (ATAx - ATAxp);
				dcopy(n,ATAx,1,ATAs,1);
				dscal(n,1.0 + beta,ATAs,1);
				daxpy(n,-beta,ATAxp,1,ATAs,1);
			}
			else{
				alpha = (-1.0 + sqrt(5.0)) / 2.0;
				beta = 0.0;
				// s = x; As = Ax; ATAs = ATAx;
				dcopy(n,x,1,s,1);
				dcopy(m,Ax,1,As,1);
				dcopy(n,ATAx,1,ATAs,1);
			}

			// g = ATAs - ATy + rsL2 * s;
			dcopy(n,ATAs,1,g,1);
			daxpy(n,-1.0,ATy,1,g,1);
			daxpy(n,rsL2,s,1,g,1);

			// v= s - g/L;
			dcopy(n,s,1,v,1);
			daxpy(n,-1.0/L , g,1,v,1);

			eplb(xnew,&root,&steps,v,n,z,lambda0);
			lambda0 = root;

			// v = xnew - s;
			dcopy(n,xnew,1,v,1);
			daxpy(n,-1.0,s,1,v,1);

			normalizedmv('N',&opt,A,xnew,Axnew);

			// Av = Axnew - As;
			dcopy(m,Axnew, 1, Av,1);
			daxpy(m, -1.0 , As,1,Av,1);

			r_sum = ddot(n,v,1,v,1);
			l_sum = ddot(m,Av,1,Av,1) + rsL2*ddot(n,v,1,v,1);

			if( l_sum <= r_sum * L)
				break;
			else
				L = maxof2( 2*L , l_sum / r_sum );
		}
		gamma = L * alpha * alpha;
		alphap = alpha;


		ValueL[iterStep] = L;

		tao = L * r_sum/l_sum;
		if(tao >= 5.0)
			L = L*0.8;

		// xp = x; x = xnew; xxp = x - xp;
		// Axp = Ax ; Ax = Axnew;
		dcopy(n,x,1,xp,1);
		dcopy(n,xnew,1,x,1);
		dcopy(n,x,1,xxp,1);
		daxpy(n,-1.0,xp,1,xxp,1);
		dcopy(m,Ax,1,Axp,1);
		dcopy(m,Axnew,1,Ax,1);

		// Axy = Ax - y;
		fValp = fVal = funVal[iterStep] =  ddot(m,Axy,1,Axy,1)/2.0 + rsL2 * ddot(n,x,1,x,1) /2.0 ;
		if(iterStep != 0)
			fValp = funVal[iterStep -1];
		norm_xxp = dnrm2(n,xxp,1);
		norm_xp = dnrm2(n,xp,1);
		if(terminationCondition(&opt,fVal,fValp,x,norm_xxp,norm_xp,iterStep))
			break;
	}

	funret.errorCode = 0;
	funret.funVal = funVal;
	funret.totIter = iterStep;
	strcpy(funret.type,"LeastC");
	funret.x = x;
	funret.ValueL = ValueL;
	return funret;


}